class robot{
  
  float[] axis = new float[] {0,0,0,0,0,0};
  float[][] base;
  float[][] axis1; 
  float[][] axis2; 
  float[][] axis3; 
  float[][] axis4; 
  float[][] axis5; 
  float[][] axis6; 
  float[] axisX = new float[] {0,0,0,0,0,0};
  float[] axisY = new float[] {0,0,0,0,0,0};
  float[] axisWidth = new float[] {0,0,0,0,0,0};
  float[] axisHeig = new float[] {0,0,0,0,0,0};
  float scaleValue = 1;
  
  int x = 0;
  int y = 0;
  int robotHeight = 0;
  int robotWidth = 0;
  
  robot(int _x, int _y, int _width, int _height){
    x = _x;
    y = _y;
    robotHeight = _width;
    robotWidth = _height;
    
    base = Math.Pos_Rot2m(new float[] {x, y, 0, 0, 0, 0});
    axis1 = Math.Pos_Rot2m(new float[] {0, -30 * scaleValue, 0, 0, 0, 0});
    axis2 = Math.Pos_Rot2m(new float[] {0, -20 * scaleValue, 0, 0, 0, 0});
    axis3 = Math.Pos_Rot2m(new float[] {0, -100 * scaleValue, 0, 0, 0, 0});
    axis4 = Math.Pos_Rot2m(new float[] {70 * scaleValue, 0, 0, 0, 0, 0});
    axis5 = Math.Pos_Rot2m(new float[] {20 * scaleValue, 0, 0, 0, 0, 0});
    axis6 = Math.Pos_Rot2m(new float[] {20 * scaleValue, 0, 0, 0, 0, 0});
    
  }
   
  void drawRobot(){
    
    // Draw base
    /*
    getAxisMatrix[0]
    line(-50*scaleValue, 0, 50*scaleValue, 0);
    line(0, 0, 0, -30*scaleValue);
    
    // Draw axis one
    translate(0, -30*scaleValue);
    rect(-7.5*scaleValue, -20*scaleValue, 15*scaleValue, 20*scaleValue);
    */
    
    pushMatrix();
    translate(x, y);
    
    // Draw base
    line(-50*scaleValue, 0, 50*scaleValue, 0);
    line(0, 0, 0, -30*scaleValue);
    
    // Draw axis one
    translate(0, -30*scaleValue);
    rect(-7.5*scaleValue, -20*scaleValue, 15*scaleValue, 20*scaleValue);
    
    translate(0, -20*scaleValue);
    line(0, 0, 0, -20*scaleValue);
    
    translate(0, -20*scaleValue);
    rotate(radians(axis[1]));
    arc(0, 0, 15*scaleValue, 15*scaleValue, 0, TWO_PI);
    line(0, -7.5*scaleValue, 0, -100*scaleValue);
    
    translate(0, -100*scaleValue);
    rotate(radians(axis[2]));
    arc(0, 0, 15*scaleValue, 15*scaleValue, 0, TWO_PI);
    line(7.5*scaleValue, 0, 70*scaleValue, 0);
    
    translate(70*scaleValue, 0);
    rotate(radians(axis[3]));
    arc(0, 0, 15*scaleValue, 15*scaleValue, 0, TWO_PI);
    line(7.5*scaleValue, 0, 20*scaleValue, 0);
    
    translate(20*scaleValue, 0);
    rect(0, -7.5*scaleValue, 20*scaleValue, 15*scaleValue);
    
    translate(20*scaleValue, 0);
    line(0, 0, 15*scaleValue, 0);
    
    popMatrix();  
  }
  
  float[][] getAxisMatrix(int axis){
    
    float[][] tempMatrix = Math.Uni2mtr();
    
    if(axis >= 0){
      tempMatrix = Math.Tform_mult(tempMatrix, base);
    }
    if(axis >= 1){
      tempMatrix = Math.Tform_mult(tempMatrix, axis1);
    }
    if(axis >= 2){
      tempMatrix = Math.Tform_mult(tempMatrix, axis2);
    }
    if(axis >= 3){
      tempMatrix = Math.Tform_mult(tempMatrix, axis3);
    }
    if(axis >= 4){
      tempMatrix = Math.Tform_mult(tempMatrix, axis4);
    }
    if(axis >= 5){
      tempMatrix = Math.Tform_mult(tempMatrix, axis5);
    }  
    if(axis >= 6){
      tempMatrix = Math.Tform_mult(tempMatrix, axis6);
    }
    
    return tempMatrix;
  }
  
  public PVector getAxisTrans(int axis){
    
    float[] temp = new float[] {0,0,0};
    
    if(axis >= 0){
      temp = Math.getMtrTrans(getAxisMatrix(0));
    }else if(axis >= 1){
      temp = Math.getMtrTrans(getAxisMatrix(1));
    }else if(axis >= 2){
      temp = Math.getMtrTrans(getAxisMatrix(2));
    }else if(axis >= 3){
      temp = Math.getMtrTrans(getAxisMatrix(3));
    }else if(axis >= 4){
      temp = Math.getMtrTrans(getAxisMatrix(4));
    }else if(axis >= 5){
      temp = Math.getMtrTrans(getAxisMatrix(5));
    }else if(axis >= 6){
      temp = Math.getMtrTrans(getAxisMatrix(6));
    }
    
    return new PVector(temp[0], temp[1]);
  }

  void updateRobot(float _axis[]){
    axis = _axis;
  }  
}
